/*
  Poor Man's Watchdog node
  Detects obstacles within a certain range and scream out

  Author: SUMo
  Version: April 17

*/

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <stereo_msgs/DisparityImage.h>
#include <std_msgs/UInt32.h>
//#include <opencv/cv.h>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#define A 0.82
#define B 0.18

#define CLEAR 0
#define ALERT 1
#define BRAKE 2

using namespace cv;
using namespace std;
using namespace image_transport;
class PoorMansWatchdog
{
private:
  ros::NodeHandle nh;
  ros::NodeHandle pn;
  ros::Subscriber image_sub;
  ros::Publisher  state_pub;
  SimpleBlobDetector detector;

  int offsetX, offsetY, rangeX, rangeY, patience;
  double cutoff, barking_value, braking_value;
  Mat element; //kernel

public:
  PoorMansWatchdog(ros::NodeHandle& n)
    :nh(n),pn("~")
  {
    image_sub = nh.subscribe("disparity",1,&PoorMansWatchdog::DIcb,this);
    state_pub = nh.advertise<std_msgs::UInt32>("camera_state",5);

    pn.param("offsetX", offsetX, 0);
    pn.param("offsetY", offsetY, 0);
    pn.param("rangeX",  rangeX,  752);
    pn.param("rangeY",  rangeY,  752);
    pn.param("distance_threshold",cutoff, 40.0);
    pn.param("barking_threshold",barking_value, 500000.0);
    pn.param("braking_threshold",braking_value, 300000.0);
    pn.param("patience", patience, 5);
    int side;
    pn.param("kernel_halfsize", side, 5);

    element = getStructuringElement(MORPH_RECT, Size(2*side+1,2*side+1),
				    Point(side, side));
  }

  void DIcb(const stereo_msgs::DisparityImagePtr& dmsg)
  {
    static std_msgs::UInt32 state;
    static double filter = 0;
    Mat_<float> dmat(dmsg->image.height, dmsg->image.width,
			(float*)&dmsg->image.data[0], dmsg->image.step);

    // create a subimage:
    Mat roi_32f(dmat, Rect(offsetX, offsetY, rangeX, rangeY));

    Mat pic;
    GaussianBlur(roi_32f, pic, Size(7,7), 0,0);
    threshold(roi_32f, pic, cutoff, 50.0, THRESH_TOZERO);

    erode( pic, pic, element);
    dilate(pic, pic, element);
    
    cv::Scalar pic_sum = cv::sum(pic);

    filter = B*pic_sum.val[0] + A*filter;

    if( filter > barking_value)
      {
	if (state.data == CLEAR)
	  {
	    state.data = ALERT;
	    //state_pub.publish(state);
	    ROS_INFO("Sum = %lf, alert", filter);
	  }
      }
    else if (filter < braking_value)
      {
	if (state.data == ALERT)
	  {
	    state.data = BRAKE;
	    state_pub.publish(state);
	    ROS_INFO("Sum = %lf, braking", filter);
	    state.data = CLEAR;
	  }
      }
    ROS_INFO("Sum = %lf", filter);
    cv::imshow("view_roi", pic);
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "poor man's watchdog");
  ros::NodeHandle n;
  cvNamedWindow("view_roi", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO);
  cvStartWindowThread();
  PoorMansWatchdog pmw(n);
  ROS_WARN("Watchdog running");
  ros::spin();
  cvDestroyWindow("view_roi");
}
